// ELbpRecognizer.cpp: implementation of the ELbpRecognizer class.
//
//////////////////////////////////////////////////////////////////////

#include "stdafx.h"
#include "IrIdentify.h"
#include "ELbpRecognizer.h"

#ifdef _DEBUG
#undef THIS_FILE
static char THIS_FILE[]=__FILE__;
#define new DEBUG_NEW
#endif

//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////

int FilterSize[TOTAL_FILTERS] = {3, 9, 15, 21, 27, 33, 39, 45, 51, 57};

unsigned char uniformFea[TOTAL_FILTERS][BINS_OF_UNIFORM_FEA] = { 
	/*filter size: 03*/	{0, 1, 2, 3, 4, 6, 7, 8, 12, 14, 15, 16, 24, 28, 30, 31, 32, 47, 48, 56, 60, 62, 63, 64, 95, 96, 112, 120, 124, 125, 126, 127, 128, 129, 131, 135, 143, 159, 191, 192, 193, 195, 199, 207, 215, 223, 224, 225, 227, 231, 239, 240, 241, 243, 245, 247, 248, 249, 251, 252, 253, 254, 255},
	/*filter size: 09*/	{0, 1, 2, 3, 4, 6, 7, 8, 12, 14, 15, 16, 24, 28, 30, 31, 32, 47, 48, 56, 60, 62, 63, 64, 96, 112, 120, 122, 124, 126, 127, 128, 129, 131, 135, 143, 158, 159, 191, 192, 193, 195, 199, 203, 207, 223, 224, 225, 227, 231, 238, 239, 240, 241, 243, 247, 248, 249, 251, 252, 253, 254, 255},
	/*filter size: 15*/	{0, 1, 2, 3, 4, 6, 7, 8, 12, 14, 15, 16, 24, 28, 30, 31, 32, 48, 52, 56, 60, 62, 63, 64, 96, 112, 120, 122, 124, 126, 127, 128, 129, 131, 135, 143, 158, 159, 191, 192, 193, 195, 199, 203, 207, 223, 224, 225, 227, 231, 238, 239, 240, 241, 243, 247, 248, 249, 251, 252, 253, 254, 255},
	/*filter size: 21*/	{0, 1, 2, 3, 4, 5, 6, 7, 8, 12, 14, 15, 16, 20, 24, 28, 30, 31, 32, 44, 48, 56, 60, 62, 63, 64, 96, 112, 120, 124, 126, 127, 128, 129, 131, 134, 135, 143, 159, 191, 192, 193, 195, 199, 207, 223, 224, 225, 227, 231, 238, 239, 240, 241, 243, 247, 248, 249, 251, 252, 253, 254, 255},
	/*filter size: 27*/	{0, 1, 2, 3, 4, 5, 6, 7, 8, 12, 14, 15, 16, 20, 24, 28, 30, 32, 44, 48, 56, 60, 62, 63, 64, 65, 68, 80, 96, 112, 120, 124, 126, 127, 128, 129, 131, 134, 135, 143, 159, 191, 192, 193, 195, 199, 207, 223, 224, 225, 227, 231, 238, 239, 240, 241, 243, 248, 249, 251, 252, 254, 255},
	/*filter size: 33*/	{0, 1, 2, 3, 4, 6, 7, 8, 12, 14, 15, 16, 24, 28, 30, 32, 44, 48, 56, 60, 62, 63, 64, 65, 80, 96, 112, 120, 124, 126, 127, 128, 129, 131, 134, 135, 143, 159, 177, 191, 192, 193, 195, 199, 207, 223, 224, 225, 227, 231, 238, 239, 240, 241, 243, 247, 248, 249, 251, 252, 253, 254, 255},
	/*filter size: 39*/	{0, 1, 3, 4, 6, 7, 8, 12, 16, 24, 28, 30, 31, 32, 33, 38, 48, 56, 60, 62, 63, 64, 65, 80, 96, 98, 111, 112, 120, 124, 126, 127, 128, 129, 131, 135, 143, 144, 159, 191, 192, 193, 195, 199, 200, 207, 223, 224, 225, 227, 231, 238, 239, 240, 241, 243, 247, 248, 249, 251, 253, 254, 255},
	/*filter size: 45*/	{0, 1, 3, 4, 6, 7, 12, 16, 24, 28, 30, 31, 32, 33, 48, 56, 60, 62, 63, 64, 65, 66, 72, 80, 96, 98, 111, 112, 114, 120, 124, 126, 127, 128, 129, 131, 133, 135, 143, 144, 192, 193, 195, 199, 200, 201, 206, 207, 222, 223, 224, 225, 237, 239, 240, 241, 243, 247, 248, 249, 253, 254, 255},
	/*filter size: 51*/	{0, 1, 3, 4, 7, 16, 24, 28, 31, 32, 33, 36, 48, 49, 56, 60, 62, 64, 65, 66, 72, 80, 96, 98, 110, 111, 112, 120, 124, 126, 127, 128, 129, 131, 132, 133, 135, 143, 144, 177, 192, 193, 194, 195, 199, 200, 201, 206, 207, 222, 223, 224, 225, 237, 239, 240, 241, 247, 248, 249, 253, 254, 255},
	/*filter size: 57*/	{0, 1, 2, 3, 4, 8, 16, 17, 24, 31, 32, 36, 44, 48, 49, 52, 60, 62, 64, 65, 66, 72, 80, 95, 96, 110, 111, 112, 120, 124, 126, 127, 128, 129, 131, 132, 133, 134, 135, 144, 177, 192, 193, 194, 195, 199, 200, 205, 206, 207, 222, 223, 224, 225, 237, 239, 240, 241, 246, 247, 248, 253, 255} 
};

int sum[3][3];

ELbpRecognizer::ELbpRecognizer()
{
	if(0)
	{
		char modelFile[256];

		char pModuleName[256];
		::GetModuleFileName(g_hModule,pModuleName,256);
		CString strTemp;
		strTemp=pModuleName;
		int pos=strTemp.ReverseFind('\\');
		CString strModuleFile=strTemp.Left(pos);
		strcpy(modelFile,strModuleFile+"\\cascadeELBP.MDL");

	//	AfxMessageBox(modelFile);
		bool b=Init(modelFile,0);
	}
	else
	{
		bool b=Init("IDR_CASCADE_ELBP896",1);
	}

//	CString str;
//	str.Format("%d",GetFeatureSize());
//	AfxMessageBox(str);


//	bool b=Init("ELBP.MDL");
}

ELbpRecognizer::~ELbpRecognizer()
{
	UnInit();
}


bool ELbpRecognizer::Init(const char* lpszModelFile,int nLoadType) 
{
	if(nLoadType==0)
	{
		if(!LoadModel(lpszModelFile)) return false;
	}
	else
	{
		if(!LoadModelFromRes(lpszModelFile)) return false;
	}

	feaVector = new unsigned char[m_Model.totalFeatures];
	if(feaVector == NULL) return false;

	intImg = new int*[IMAGE_HEIGHT];
	if(intImg == NULL) return false;
	intImg[0] = new int[IMAGE_HEIGHT * IMAGE_WIDTH];
	if(intImg == NULL) return false;
	for(int i=1; i<IMAGE_HEIGHT; ++i) intImg[i] = intImg[i-1] + IMAGE_WIDTH;

	pFImg = new unsigned char**[TOTAL_FILTERS];
	if (pFImg == NULL) throw string("Memory overflow while init creators!");
	pFImg[0] = new unsigned char*[TOTAL_FILTERS * IMAGE_HEIGHT];
	if (pFImg[0] == NULL) throw string("Memory overflow while init creators!");
	pFImg[0][0] = new unsigned char[TOTAL_FILTERS * IMAGE_HEIGHT * IMAGE_WIDTH];
	if (pFImg[0][0] == NULL) throw string("Memory overflow while init creators!");
	for (int k=0; k<TOTAL_FILTERS; ++k)	{
		if (k>0) {
			pFImg[k] = pFImg[k-1] + IMAGE_HEIGHT;
			pFImg[k][0] = pFImg[k-1][0] + IMAGE_HEIGHT*IMAGE_WIDTH;
		}
		for (int i=1; i<IMAGE_HEIGHT; ++i) {
			pFImg[k][i] = pFImg[k][i-1] + IMAGE_WIDTH;
		}
	}

	return true;
}


bool ELbpRecognizer::UnInit() 
{
	for(int i=0; i<m_Model.totalStages; ++i) 
	{
		delete[] m_Model.SC[i].fea;
	}

	delete[] m_Model.SC;
	delete[] feaVector;
	delete[] intImg[0];
	delete[] intImg;
	delete[] pFImg[0][0];
	delete[] pFImg[0];
	delete[] pFImg;

	return true;
}


BYTE* ELbpRecognizer::GetFeatures(unsigned char *pImg) 
{
	int feaId, subWinSize, filterID;
	int r0, c0, r1, c1, r, c;
	unsigned char binID;
	float N;

	Integal(pImg);
	ELBPFilter();

	feaId = 0;
	for(int i=0; i<m_Model.totalStages; ++i) {
		for(int j=0; j<m_Model.SC[i].totalWCs; ++j) {
			subWinSize = m_Model.SC[i].fea[j].subWinSize;
			filterID = m_Model.SC[i].fea[j].filterID;
			binID = m_Model.SC[i].fea[j].binID;

			r0 = m_Model.SC[i].fea[j].row;
			c0 = m_Model.SC[i].fea[j].col;
			r1 = r0 + subWinSize;
			c1 = c0 + subWinSize;

			N = 0;

			for(r=r0; r<r1; ++r) {
				for(c=c0; c<c1; ++c) {						
					if(pFImg[filterID][r][c] == binID) ++N;	
				}
			}

			N *= 320.0 / (subWinSize * subWinSize); 
			if(N > 255) N = 255;
			
			feaVector[feaId++] = (unsigned char)N;
		}
	}

	return feaVector;
}


double ELbpRecognizer::CompareTwoFeatures(unsigned char *pFea1, unsigned char *pFea2) 
{
/*	int feaId = 0;
	int dist;
	Feature *fea;
	double score, tempScore;

	score = 0;
	
	for(int i=0; i<m_Model.totalStages;++i) 
	{
		tempScore = 0;

		for(int j=0; j<m_Model.SC[i].totalWCs; ++j) 
		{
			dist = pFea1[feaId] - pFea2[feaId];
			if(dist < 0) dist = -dist;

			fea = m_Model.SC[i].fea + j;

			if(dist <= fea->threshold) tempScore += fea->lval; 
			else tempScore += fea->rval;

			++feaId;
		}

		tempScore = (tempScore - m_Model.SC[i].minScore / 4) / (m_Model.SC[i].maxScore - m_Model.SC[i].minScore / 4);
		if(tempScore < 0) tempScore = 0;

		score += tempScore;
	}

	return score; //Threshold at FAR0.1% : 0.4718874
*/
	int feaId, dist, passedStages;
	double score, tempScore, Fx;
	Feature *fea;
	
	score = 0;
	feaId = 0;
	passedStages = 0;
	
	for(int i=0; i<m_Model.totalStages; ++i)
	{
		Fx = 0;
		
		for(int j=0; j<m_Model.SC[i].totalWCs; ++j) 
		{
			dist = pFea1[feaId] - pFea2[feaId];
			if(dist < 0) dist = -dist;
			
			fea = m_Model.SC[i].fea + j;
			
			if(dist <= fea->threshold) Fx += fea->lval; 
			else Fx += fea->rval;
			
			++feaId;
		}
		
		tempScore = (Fx - m_Model.SC[i].minScore / 4) / (m_Model.SC[i].maxScore - m_Model.SC[i].minScore / 4);
		if(tempScore < 0) tempScore = 0;
		
		score += tempScore;
		++passedStages;
		
		if(Fx < m_Model.SC[i].stageThr) break;
	}
	
	return score / passedStages;	

}


bool ELbpRecognizer::LoadModel(const char *modelFile) 
{

	double tempMax, tempMin, lval, rval;

	ifstream fin(modelFile);
	if(fin.fail()) return false;
	
	fin >> m_Model.totalStages;
	if(fin.fail()) return false;
	
	m_Model.SC = new StageClassifier[m_Model.totalStages];
	if(m_Model.SC == NULL) return false;
	
	m_Model.totalFeatures = 0;
	for(int i=0; i<m_Model.totalStages; ++i) {
		fin >> m_Model.SC[i].totalWCs >> m_Model.SC[i].stageThr;
		if(fin.fail()) return false;
		
		m_Model.totalFeatures += m_Model.SC[i].totalWCs;
		
		m_Model.SC[i].fea = new Feature[m_Model.SC[i].totalWCs];
		if(m_Model.SC[i].fea == NULL) return false;

		tempMax = 0;
		tempMin = 0;

		for(int j=0; j<m_Model.SC[i].totalWCs; ++j) {
			fin >> m_Model.SC[i].fea[j].feaId
				>> m_Model.SC[i].fea[j].filterID
				>> m_Model.SC[i].fea[j].filterSize
				>> m_Model.SC[i].fea[j].subWinSize
				>> m_Model.SC[i].fea[j].row
				>> m_Model.SC[i].fea[j].col
				>> m_Model.SC[i].fea[j].binID
				>> m_Model.SC[i].fea[j].threshold
				>> lval
				>> rval;
			
			if(fin.fail()) return false;
			
			m_Model.SC[i].fea[j].lval = lval;
			m_Model.SC[i].fea[j].rval = rval;

			if(lval >= rval) {
				tempMax += lval;
				tempMin += rval;
			}
			else {
				tempMax += rval;
				tempMin += lval;
			}			
		}

		m_Model.SC[i].maxScore = tempMax;
		m_Model.SC[i].minScore = tempMin;
	}
	
	fin.close();
	return true;
}

bool ELbpRecognizer::LoadModelFromRes(const char *ModelRes)
{
	double tempMax, tempMin, lval, rval;

	char pModuleName[256];
	::GetModuleFileName(g_hModule,pModuleName,256);

	LPVOID pvModelData=LoadDataResource(ModelRes,pModuleName);

	if(pvModelData)
	{
	}
	else
	{
		return false;
	}

	istrstream fin( (char*)pvModelData );

	///////////////////////
	if(fin.fail()) return false;
	
	fin >> m_Model.totalStages;
	if(fin.fail()) return false;
	
	m_Model.SC = new StageClassifier[m_Model.totalStages];
	if(m_Model.SC == NULL) return false;
	
	m_Model.totalFeatures = 0;
	for(int i=0; i<m_Model.totalStages; ++i) {
		fin >> m_Model.SC[i].totalWCs >> m_Model.SC[i].stageThr;
		if(fin.fail()) return false;
		
		m_Model.totalFeatures += m_Model.SC[i].totalWCs;
		
		m_Model.SC[i].fea = new Feature[m_Model.SC[i].totalWCs];
		if(m_Model.SC[i].fea == NULL) return false;

		tempMax = 0;
		tempMin = 0;

		for(int j=0; j<m_Model.SC[i].totalWCs; ++j) {
			fin >> m_Model.SC[i].fea[j].feaId
				>> m_Model.SC[i].fea[j].filterID
				>> m_Model.SC[i].fea[j].filterSize
				>> m_Model.SC[i].fea[j].subWinSize
				>> m_Model.SC[i].fea[j].row
				>> m_Model.SC[i].fea[j].col
				>> m_Model.SC[i].fea[j].binID
				>> m_Model.SC[i].fea[j].threshold
				>> lval
				>> rval;
			
			if(fin.fail()) return false;
			
			m_Model.SC[i].fea[j].lval = lval;
			m_Model.SC[i].fea[j].rval = rval;

			if(lval >= rval) {
				tempMax += lval;
				tempMin += rval;
			}
			else {
				tempMax += rval;
				tempMin += lval;
			}			
		}

		m_Model.SC[i].maxScore = tempMax;
		m_Model.SC[i].minScore = tempMin;
	}
	
//		fin.close();
	return true;
}
void ELbpRecognizer::Integal(unsigned char *pImg) 
{
	int i, j;

	for (i=0; i<IMAGE_HEIGHT; ++i) 
	{
		intImg[i][0] = pImg[i * IMAGE_WIDTH];

		for (j=1; j<IMAGE_WIDTH; ++j) 
			intImg[i][j] = intImg[i][j-1] + pImg[i * IMAGE_WIDTH + j];
	}
		
	for (j=0; j<IMAGE_WIDTH; ++j) 
		for (i=1; i<IMAGE_HEIGHT; ++i) 
			intImg[i][j] += intImg[i-1][j];			
}


void ELbpRecognizer::ELBPFilter() 
{	
	bitset<8> b;
	unsigned char fval, *p, *pUF0, *pUF1;
	int offset, s, i, j, m, n, m1, n1, m2, n2, A, B, C, D;

	for(int filterID=0; filterID<TOTAL_FILTERS; ++filterID) {
		s = FilterSize[filterID] / 3;
		offset = FilterSize[filterID] / 2;		
		pUF0 = uniformFea[filterID];
		pUF1 = uniformFea[filterID + 1];
		
		for(i=offset; i<IMAGE_HEIGHT-offset; ++i) {
			for(j=offset; j<IMAGE_WIDTH-offset; ++j) {			
				
				for(m=0; m<3; m++) {
					if(m == 0) m1 = i - offset - 1;
					else m1 += s;
					
					m2 = m1 + s;
					
					for(n=0; n<3; n++) {
						if(n == 0) n1 = j - offset - 1;
						else n1 += s;
						
						n2 = n1 + s;		
						
						A = (m1<0 || n1<0) ? 0 : intImg[m1][n1];
						B = (m1<0) ? 0 : intImg[m1][n2];
						C = (n1<0) ? 0 : intImg[m2][n1];
						D = intImg[m2][n2];		
						
						sum[m][n] = A + D - B - C;							
					}
				}
				
				b[7] = (sum[0][0] >= sum[1][1]) ? 1 : 0;
				b[6] = (sum[0][1] >= sum[1][1]) ? 1 : 0;
				b[5] = (sum[0][2] >= sum[1][1]) ? 1 : 0;
				b[4] = (sum[1][2] >= sum[1][1]) ? 1 : 0;
				b[3] = (sum[2][2] >= sum[1][1]) ? 1 : 0;
				b[2] = (sum[2][1] >= sum[1][1]) ? 1 : 0;
				b[1] = (sum[2][0] >= sum[1][1]) ? 1 : 0;
				b[0] = (sum[1][0] >= sum[1][1]) ? 1 : 0;
				
				fval = (unsigned char)b.to_ulong();
				p = lower_bound(pUF0, pUF1, fval);
				pFImg[filterID][i][j] = (fval < *p) ? BINS_OF_UNIFORM_FEA : (p - pUF0);
			}
		}
	}
}


int ELbpRecognizer::GetFeatureSize()
{
	return m_Model.totalFeatures;
}